﻿using System;
using System.Collections.Generic;
using btl.generic;
using NeuronDotNet.Core;
using NeuronDotNet.Core.Backpropagation;
using NeuronDotNet.Core.Initializers;

namespace Eater
{
    class EaterModel
    {
        // Status
        public EaterStatus Status { get; set; }
        public Vector2 Position { get; set; }
        public double Velocity { get; private set; }
        double _heading; // 0 - left, 1 - right
        public double Heading
        {
            get { return _heading; }
            set
            {
                _heading = value % 1.0;
                if (_heading <= 0.0)
                {
                    _heading += 1.0;
                }
            }
        }
        public double TargetHeading { get; set; }

        // Food
        public Food TargetFood
        {
            get { return GetClosestFood(); }
        }
        public int LastFood { get; set; }
        IList<Food> foods;

        // NN & genetics
        public Network NeuralNetwork { get; private set; }
        public double Fitness { get; set; }
        Genome _genome;
        public Genome Genome
        {
            get
            {
                return _genome;
            }
            set
            {
                _genome = value;
                if (value != null)
                {
                    SetWeights(value.Genes);
                }
            }
        }
        public int NumberOfWeights
        {
            get
            {
                if (NeuralNetwork == null)
                {
                    return 0;
                }
                int numweights = 0;
                foreach (BackpropagationConnector c in NeuralNetwork.Connectors)
                {
                    numweights += c.SynapseCount;
                }
                return numweights;
            }
        }

        public EaterModel(IList<Food> foods)
        {
            Status = EaterStatus.Full;
            TargetHeading = 0;
            LastFood = 0;
            this.foods = foods;


            const int neuronCount = 2;
            const int hiddenLayerCount = 1;
            float learningRate = 0.1f;

            LinearLayer inputLayer = new LinearLayer(3);
            inputLayer.UseFixedBiasValues = false;
            LinearLayer outputLayer = new LinearLayer(2);
            outputLayer.UseFixedBiasValues = false;

            ActivationLayer[] hiddenLayers = new ActivationLayer[hiddenLayerCount];
            for (int i = 0; i < hiddenLayerCount; i++)
            {
                hiddenLayers[i] = new LinearLayer(neuronCount);
                hiddenLayers[i].UseFixedBiasValues = false;
                if (i == 0)
                {
                    new BackpropagationConnector(inputLayer, hiddenLayers[0]).Initializer = new RandomFunction(-1, 1);
                }
                else
                {
                    new BackpropagationConnector(hiddenLayers[i - 1], hiddenLayers[i]).Initializer = new RandomFunction(-1, 1);
                }

                if (i == hiddenLayerCount - 1)
                {
                    new BackpropagationConnector(hiddenLayers[i], outputLayer).Initializer = new RandomFunction(-1, 1);
                }
            }

            NeuralNetwork = new BackpropagationNetwork(inputLayer, outputLayer);
            NeuralNetwork.SetLearningRate(learningRate);
        }

        public double DistanceTo(Food food)
        {
            return Vector2.Distance(Position, food.Position);
        }

        /// <summary>
        /// Gets angle between up vector and food vector
        /// </summary>
        /// <param name="food"></param>
        /// <returns>Angle in radians</returns>
        public double AngleTo(Vector2 to, Vector2 position)
        {
            Vector2 toVector = to - position;
            Vector2 upVector = new Vector2(0, 1);
            double heading = toVector.AngleBetween(upVector);
            if (toVector.X < 0)
            {
                heading = -heading;
            }
            return heading;
        }

        public Food GetClosestFood()
        {
            double distanceSqMin = double.MaxValue;
            int distanceSqMinIndex = -1;
            for (int j = 0; j < foods.Count; j++)
            {
                double distanceSq = Vector2.DistanceSquared(foods[j].Position, Position);
                if (distanceSq < distanceSqMin)
                {
                    distanceSqMin = distanceSq;
                    distanceSqMinIndex = j;
                }
            }

            if (distanceSqMinIndex == -1)
            {
                // Corner case angle calculation error, return first
                return foods[0];
            }

            return foods[distanceSqMinIndex];
        }

        public void Update()
        {
            LastFood++;

            if (LastFood == 100)
            {
                Status = EaterStatus.Hungry;
            }
            else if (LastFood == 200)
            {
                Status = EaterStatus.Dying;
            }
            else if (LastFood == 700)
            {
                //Status = EaterStatus.Dead;
            }

            // Run the neural network
            double distanceToFood = Math.Min(DistanceTo(TargetFood), 100.0) / 100.0; // Map 0...100 to 0...1
            double foodHeading = AngleTo(TargetFood.Position, Position);
            double angleToFood = (foodHeading / (2 * Math.PI)) + 0.5; // Map -pi...pi to 0...1
            double deltaAngle = angleToFood - Heading;
            if (deltaAngle > 0.5)
            {
                deltaAngle = -1.0 + deltaAngle;
            }
            else if (deltaAngle < -0.5)
            {
                deltaAngle = deltaAngle + 1.0;
            }
            //Console.WriteLine("{0:0.0000}", distanceToFood);
            const double bias = 1.0;
            double[] inputs = new double[] { distanceToFood, angleToFood, bias };
            //double[] inputs = new double[] { angleToFood };
            double[] outputs = NeuralNetwork.Run(inputs);

            Velocity = outputs[1];
            Velocity = Math.Max(Velocity, 0.0);

            double wantedHeading = outputs[0];
            // TODO: The faster the eater, the slower it turns
            //Heading = Heading + (angleToFood - Heading) * (1.0 - Velocity); // Lerp
            Heading = wantedHeading;
            //Heading = angleToFood;

            //Console.WriteLine(Heading);

            double velocity = Velocity * 2; // 0...1 to 0...2

            Vector2 headingVector = Vector2.Multiply(Vector2.GetHeadingVector(Heading), velocity);
            Position += headingVector;
        }

        // Set neural network weights
        void SetWeights(double[] weights)
        {
            int index = 0;

            foreach (BackpropagationConnector connector in NeuralNetwork.Connectors)
            {
                foreach (BackpropagationSynapse synapse in connector.Synapses)
                {
                    synapse.Weight = weights[index++];
                }
            }
       }
    }
}
